/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : app_freertos.c
  * Description        : Code for freertos applications
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2023 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */

/* Includes ------------------------------------------------------------------*/
#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "adc.h"
#include "usart.h"
#include "tim.h"

#include <stdio.h>
#include <stdbool.h>

/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */

#define CH_COUNT 6
struct Frame
{
    float fdata[CH_COUNT];
    unsigned char tail[4];
};

struct Frame vfoaFrame = {
        .tail = {0x00, 0x00, 0x80, 0x7f}
};

struct motor
{
    float ia, ib, ic;
};
struct motor motor;


/* USER CODE END Variables */
/* Definitions for defaultTask */
osThreadId_t defaultTaskHandle;
const osThreadAttr_t defaultTask_attributes = {
        .name = "defaultTask",
        .priority = (osPriority_t) osPriorityNormal,
        .stack_size = 128 * 4
};

/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */

/* USER CODE END FunctionPrototypes */

void StartDefaultTask(void *argument);

void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */

/**
  * @brief  FreeRTOS initialization
  * @param  None
  * @retval None
  */
void MX_FREERTOS_Init(void)
{
    /* USER CODE BEGIN Init */
    HAL_ADCEx_Calibration_Start(&hadc1, ADC_SINGLE_ENDED);
    HAL_ADCEx_Calibration_Start(&hadc2, ADC_SINGLE_ENDED);

    HAL_ADCEx_InjectedStart_IT(&hadc1);
    HAL_ADCEx_InjectedStart(&hadc2);

    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_4);

    /* USER CODE END Init */

    /* USER CODE BEGIN RTOS_MUTEX */
    /* add mutexes, ... */
    /* USER CODE END RTOS_MUTEX */

    /* USER CODE BEGIN RTOS_SEMAPHORES */
    /* add semaphores, ... */
    /* USER CODE END RTOS_SEMAPHORES */

    /* USER CODE BEGIN RTOS_TIMERS */
    /* start timers, add new ones, ... */
    /* USER CODE END RTOS_TIMERS */

    /* USER CODE BEGIN RTOS_QUEUES */
    /* add queues, ... */
    /* USER CODE END RTOS_QUEUES */

    /* Create the thread(s) */
    /* creation of defaultTask */
    defaultTaskHandle = osThreadNew(StartDefaultTask, NULL, &defaultTask_attributes);

    /* USER CODE BEGIN RTOS_THREADS */
    /* add threads, ... */
    /* USER CODE END RTOS_THREADS */

    /* USER CODE BEGIN RTOS_EVENTS */
    /* add events, ... */
    /* USER CODE END RTOS_EVENTS */

}

/* USER CODE BEGIN Header_StartDefaultTask */
/**
  * @brief  Function implementing the defaultTask thread.
  * @param  argument: Not used
  * @retval None
  */
/* USER CODE END Header_StartDefaultTask */
void StartDefaultTask(void *argument)
{
    /* USER CODE BEGIN StartDefaultTask */
    /* Infinite loop */
    for (;;)
    {
        osDelay(1);
    }
    /* USER CODE END StartDefaultTask */
}

/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */


/**
  * @brief  Injected conversion complete callback in non-blocking mode.
  * @param hadc ADC handle
  * @retval None
  */
void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *hadc)
{
    uint32_t ia, ib, ic;
    static bool calcAdcOffsetOver = false;
    static float iaOffset = 0, ibOffset = 0, icOffset = 0;

    const int measCnt = 16;
    static int measCntCopy = measCnt;
    if (hadc->Instance == ADC1 && !calcAdcOffsetOver)
    {
        ia = HAL_ADCEx_InjectedGetValue(&hadc1, ADC_INJECTED_RANK_1);
        ic = HAL_ADCEx_InjectedGetValue(&hadc1, ADC_INJECTED_RANK_2);
        ib = HAL_ADCEx_InjectedGetValue(&hadc2, ADC_INJECTED_RANK_1);
        iaOffset += (float) ia;
        icOffset += (float) ic;
        ibOffset += (float) ib;

        if (--measCntCopy <= 0)
        {
            iaOffset /= (float) (measCnt * 1.0);
            icOffset /= (float) (measCnt * 1.0);
            ibOffset /= (float) (measCnt * 1.0);

            calcAdcOffsetOver = true;
        }
    }

    if (hadc->Instance == ADC1 && calcAdcOffsetOver)
    {
        ia = HAL_ADCEx_InjectedGetValue(&hadc1, ADC_INJECTED_RANK_1);
        ic = HAL_ADCEx_InjectedGetValue(&hadc1, ADC_INJECTED_RANK_2);
        ib = HAL_ADCEx_InjectedGetValue(&hadc2, ADC_INJECTED_RANK_1);
        motor.ia = /*((float) ia - iaOffset) * 0.02197f*/(float) (ia * 1.0);
        motor.ic = /*((float) ic - iaOffset) * 0.02197f*/(float) (ic * 1.0);
        motor.ib = /*((float) ib - ibOffset) * 0.02197f*/(float) (ib * 1.0);

        vfoaFrame.fdata[0] = iaOffset;
        vfoaFrame.fdata[1] = ibOffset;
        vfoaFrame.fdata[2] = icOffset;
        vfoaFrame.fdata[3] = motor.ia;
        vfoaFrame.fdata[4] = motor.ib;
        vfoaFrame.fdata[5] = motor.ic;
        HAL_UART_Transmit_DMA(&huart3, (uint8_t *) (&vfoaFrame), sizeof(vfoaFrame));
    }
}


/* USER CODE END Application */

